#include <pic18fregs.h>
#include <pic16/adc.h>
#include <stdlib.h>
#include <stdio.h>
#include <usart.h>
#include "kodak_timer.h"

#define DEBUG
//#define USE_LED_FOR_LAMP_DEBUG
//#define USE_LED_FOR_REMOTE_DEBUG
#define USE_LED_FOR_QUEUE_FULL_DEBUG
//#define USE_LED_FOR_AD_TEST
//#define USE_LED_FOR_USART_DEBUG

#define QUEUE_SIZE 256

// Global variables
unsigned char dimmer_count;	/* Interrupt Counter */
unsigned char dimmer_level;	/* Target Level      */
unsigned char max_dimmer_level;
unsigned int dimmer_time;
volatile unsigned char ad0, ad1;
unsigned long timer_2;
unsigned int timer_1_soft_postscaler;
direction_t direction;
dimmer_direction_t dimmer_direction;
unsigned char dias_num;
unsigned char dias_position;

// command queue
volatile unsigned int fifo_head, fifo_tail;
volatile command_t fifo_buffer[QUEUE_SIZE];
volatile command_t command;
volatile unsigned char id;

unsigned char eeprom_id;

void main(void) {
	OSCCONbits.SCS = 0x0;			// System Clock Select bits = External oscillator
	OSCCONbits.IRCF = 0x7;		// Internal Oscillator Frequency Select bits 8 MHz (INTOSC drives clock directly)

	TRIS_ZERO_CROSS_DETECTOR_PIN = 1;
	TRIS_BACKWARD_PIN = 0;
	TRIS_FORWARD_PIN = 0;
	TRIS_LAMP_PIN  = 0;
	TRIS_DIAS_DETECTOR_PIN = 1;
	TRIS_LED = 0;

	TRIS_AD_0 = 1;
	TRIS_AD_1 = 1;

	TRIS_TX_PIN = 0;
	TRIS_RX_PIN = 1;

	BACKWARD_PIN = 0;
	FORWARD_PIN = 0;
	LAMP_PIN = 0;
	LED_PIN = 0;

	timer_2 = 0;
	timer_1_soft_postscaler = 0;
	dimmer_level = 0;
	dimmer_time = 0;
	max_dimmer_level = MAX_DIMMER_LEVEL;
	
	dias_num = 0;
	dias_position = 0;

	fifo_head = 0;
	fifo_tail = 0;

	// EEPROP stuff
//	eeprom_id = read_EEPROM(0);
	eeprom_id = 114;
//	if ((eeprom_id == 0) || (eeprom_id == 0xff)) {
//		write_EEPROM(0, 118);
//	}

	// set up ad
	adc_open(2, ADC_FOSC_64, ADC_CFG_2A, ADC_FRM_RJUST | ADC_INT_OFF);

	// set up interrupt and timers
	RCONbits.IPEN = 1;

	// USART interrupt low priority
	IPR1bits.RCIP = 0;
	IPR1bits.TXIP = 0;

	// timer 0
	T0CONbits.TMR0ON = 0;
	T0CONbits.T08BIT = 0;	// use timer0 16-bit counter
	T0CONbits.T0CS = 0;		// internal clock source
	T0CONbits.PSA = 1;		// disable timer0 prescaler
	INTCON2bits.TMR0IP = 1;	// high priority

	// timer 1
	T1CONbits.TMR1ON = 0;
	T1CONbits.RD16 = 1;
	T1CONbits.TMR1CS = 0;	// internal clock source
	T1CONbits.T1OSCEN = 0;	// dont put t1 on pin
	T1CONbits.T1CKPS0 = 1;
	T1CONbits.T1CKPS1 = 1;
	IPR1bits.TMR1IP = 0;		// low priority

	// timer 2
	T2CONbits.T2CKPS0 = 1;
	T2CONbits.T2CKPS1 = 0;
	T2CONbits.TOUTPS0 = 1;
	T2CONbits.TOUTPS1 = 0;
	T2CONbits.TOUTPS2 = 0;
	T2CONbits.TOUTPS3 = 1;
	IPR1bits.TMR2IP = 0;		// low priority
	PIR1bits.TMR2IF = 1;
	T2CONbits.TMR2ON = 1;
	PIE1bits.TMR2IE = 1;

	// timer 3
	T3CONbits.RD16 = 1;
	T3CONbits.TMR3CS = 0;	// internal clock source
	T3CONbits.T3CKPS0 = 1;
	T3CONbits.T3CKPS0 = 1;
	IPR2bits.TMR3IP = 0;		// low priority
	T3CONbits.TMR3ON = 1;
	PIE2bits.TMR3IE = 1;
	PIR2bits.TMR3IF = 1;

	INTCONbits.INT0IE = 0;	// zero-cross interrupt - enabled when we dimm in
													// Note: T0IE enable from Zero Cross Routine
	INTCONbits.PEIE = 1;
	INTCONbits.GIE = 1;	/* Enable Global interrupts   */	

/*
	usart_open(	USART_TX_INT_OFF &
				USART_RX_INT_ON & 
				USART_BRGH_LOW & 
				USART_ASYNCH_MODE & 
				USART_EIGHT_BIT &
				USART_CONT_RX,
				129     // 9600 kbps
	);
*/

	my_usart_open();

/*
	sleep_ms(100);
	sprintf(debug_buf, "Hardware ID: '%d'", eeprom_id);
	usart_puts(debug_buf);
	sleep_ms(500);
*/

	sync_dias(1);

	while (1) {
		wait_for_command();
		switch (command) {
			case CMD_SYNC:
				sync_dias(1);
				break;
			case CMD_FORWARD:
				forward_dias();
				break;
			case CMD_BACKWARD:
				backward_dias();
				break;
			case CMD_FADE_IN:
				fade_in();
				break;
			case CMD_FADE_OUT:
				fade_out();
				break;
		}
	}
}

void sleep_ms(unsigned long ms) {
	unsigned long start_timer_2;
	start_timer_2 = timer_2;	

// while the absolute value of the time diff < ms
	while ( (((signed long)(timer_2 - start_timer_2) < 0) ? (-1 * (timer_2 - start_timer_2)) : (timer_2 - start_timer_2)) < ms) {
		// do nothing
	}
}

void fade_in(void) {		// in 4 seconds
#ifdef DEBUG
#ifdef USE_LED_FOR_LAMP_DEBUG
	LED_PIN = 1;
#endif
#endif
	INTCONbits.INT0IE = 1;	// enable zero-cross
	dimmer_direction = UP;
	PIR1bits.TMR1IF = 1;	/* Force Instant entry to Timer 1 Interrupt */
	T1CONbits.TMR1ON = 1;	// enable timer1
	PIE1bits.TMR1IE = 1;	/* Ensure that TMR1 Interrupt is enabled    */
	while (dimmer_level < max_dimmer_level) {
		// wait
	}
}

void fade_out(void) {		// in 4 seconds
#ifdef DEBUG
#ifdef USE_LED_FOR_LAMP_DEBUG
	LED_PIN = 0;
#endif
#endif
	dimmer_direction = DOWN;
	PIR1bits.TMR1IF = 1;	/* Force Instant entry to Timer 1 Interrupt */
	T1CONbits.TMR1ON = 1;	// enable timer1
	PIE1bits.TMR1IE = 1;	/* Ensure that TMR1 Interrupt is enabled    */
	while (dimmer_level > 0) {
		// wait
	}
	INTCONbits.INT0IE = 0;	// disable zero-cross
}

void forward_dias(void) {
	unsigned char i;
	if (DIAS_DETECTOR_PIN) {						// if we are out of sync, sync again to last position
		sync_dias(dias_position);
	}

	// change
	if (dias_position == dias_num) {
		for (i = 0; i < (dias_num - 1); i++) {	// and go back to first slide again
			BACKWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			BACKWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
		}
		dias_position = 1;
	}
	else {
		FORWARD_PIN = 1;
		sleep_ms(NEXT_DIAS_TIME);
		FORWARD_PIN = 0;
		sleep_ms(DIAS_CHANGE_TIME);
		dias_position++;
	}

	if (DIAS_DETECTOR_PIN) {						// if we are out of sync, sync again to last position
		sync_dias(dias_position);
	}
}

void backward_dias(void) {
	unsigned char i;
	if (DIAS_DETECTOR_PIN) {						// if we are out of sync, sync again to last position
		sync_dias(dias_position);
	}

	// change
	if (dias_position == 1) {
		for (i = 0; i < (dias_num - 1); i++) {	// and go back to first slide again
			FORWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			FORWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
		}
		dias_position = dias_num;
	}
	else {
		BACKWARD_PIN = 1;
		sleep_ms(NEXT_DIAS_TIME);
		BACKWARD_PIN = 0;
		sleep_ms(DIAS_CHANGE_TIME);
		dias_position--;
	}

	if (DIAS_DETECTOR_PIN) {						// if we are out of sync, sync again to last position
		sync_dias(dias_position);
	}
}

void sync_dias(unsigned char pos) {
	unsigned char i;
	unsigned char last_dimmer_level;

	// save dimmer level
	last_dimmer_level = dimmer_level;
	fade_out();

	if (DIAS_DETECTOR_PIN) {						// we are outside slide array
		while (DIAS_DETECTOR_PIN) {				// go forward to first slide
			FORWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			FORWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
		}
	}
	else {															// we are inside slide array
		while (!DIAS_DETECTOR_PIN) {			// go back to first empty slide
			BACKWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			BACKWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
		}
		// and go forward to first slide
		FORWARD_PIN = 1;
		sleep_ms(NEXT_DIAS_TIME);
		FORWARD_PIN = 0;
		sleep_ms(DIAS_CHANGE_TIME);
	}
	for (i = 0; i < (pos - 1); i++) {		// and go to slide at pos again
		FORWARD_PIN = 1;
		sleep_ms(NEXT_DIAS_TIME);
		FORWARD_PIN = 0;
		sleep_ms(DIAS_CHANGE_TIME);
	}
	dias_position = pos;
	direction = FORWARD;

	if (!dias_num) {										// if first time count the slides
		while (!DIAS_DETECTOR_PIN) {			// go forward to last slide
			FORWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			FORWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
			dias_num++;
		}
		for (i = 0; i < dias_num ; i++) {	// and go back to first slide
			BACKWARD_PIN = 1;
			sleep_ms(NEXT_DIAS_TIME);
			BACKWARD_PIN = 0;
			sleep_ms(DIAS_CHANGE_TIME);
		}
	}

	// restore dimmer level
	dimmer_level = last_dimmer_level;
	if (dimmer_level) {
		fade_in();
	}
}

static void phase_control(void) __interrupt 1 {
	// zero cross interrupt
	if (INTCONbits.INT0IF) {
		INTCONbits.INT0IF = 0;		/* Clear Interrupt Flag */
		dimmer_count = 0xff - dimmer_level;	/* Load Level Setting   */
		LAMP_PIN = 0;		/* Turn Output Off */
		INTCONbits.TMR0IF = 1;	/* Force Instant entry to Timer 0 Interrupt */
		T0CONbits.TMR0ON = 1;	// enable timer0
		INTCONbits.T0IE = 1;	/* Ensure that TMR0 Interrupt is enabled    */
	}
	// timer overflow interrupt
	if (INTCONbits.TMR0IF) {
		TMR0H = (unsigned char)(TIMER0_RELOAD >> 8);
		TMR0L = (unsigned char)TIMER0_RELOAD;	/* Reload the Timer ASAP */
		INTCONbits.TMR0IF = 0;	/* Clear the Timer Flag  */

		if(dimmer_count == 0) {
			LAMP_PIN = 1;	/* Turn Output ON  */
		}
		else {
			dimmer_count--;	/* Keep Decreasing Till Zero */
		}
	}
}

static void timer_control(void) __interrupt 2 {
	if (PIR1bits.TMR1IF) {
		TMR1H = (unsigned char)(TIMER1_RELOAD >> 8);	// 8 ms delay at 8 MHz
		TMR1L = (unsigned char)TIMER1_RELOAD;
		timer_1_soft_postscaler++;
		if (timer_1_soft_postscaler >= dimmer_time) {
			timer_1_soft_postscaler = 0;
			PIR1bits.TMR1IF = 0;	/* Clear the Timer Flag  */
			if ((dimmer_direction == UP) && (dimmer_level < max_dimmer_level)) {	// if we dimm
				dimmer_level++;
			}
			else if ((dimmer_direction == DOWN) && (dimmer_level > 0)) {
				dimmer_level--;
			}
			else {																	// else disable timer
				T1CONbits.TMR1ON = 0;
				PIE1bits.TMR1IE = 0;
			}
		}
	}
	if (PIR1bits.TMR2IF) {
		PR2 = TIMER2_RELOAD;		// 1 ms delay at 8 MHz
		PIR1bits.TMR2IF = 0;
		timer_2++;
		
#ifdef DEBUG
#ifdef USE_LED_FOR_QUEUE_FULL_DEBUG
		if (fifo_in_use()) {
			LED_PIN = 1;
		}
		else {
			LED_PIN = 0;
		}
#endif
#ifdef USE_LED_FOR_AD_TEST
		if ((ad0 + ad1) > 240) {
			LED_PIN = 1;
		}
		else {
			LED_PIN = 0;
		}
#endif
#endif
	}
	if (PIR2bits.TMR3IF) {
		TMR3H = (unsigned char)(TIMER3_RELOAD >> 8);	// 8 ms delay at 8 MHz
		TMR3L = (unsigned char)TIMER3_RELOAD;
		PIR2bits.TMR3IF = 0;	/* Clear the Timer Flag  */

		// AD0
		adc_setchannel(0);
		adc_conv();
		while(adc_busy()) {
			// wait
		}
		ad0 = 0xff - (adc_read() >> 2);		// potmetrene vender omvendt på boardet
		dimmer_time = (ad0 * 0x10);
		max_dimmer_level = ad1;
		
		// AD1
		adc_setchannel(1);
		adc_conv();
		while(adc_busy()) {
			// wait
		}
		ad1 = 0xff - (adc_read() >> 2);		// potmetrene vender omvendt på boardet
	}
	// serial rx interrupt
	if (usart_drdy()) {
		// retransmit it
		command = usart_getc();
		usart_putc(command);
		// and if it is for me or all put it in the queue
//		id = (command & 0xf8) >> 5; // whats wrong here?
		id = 0;	// DEBUG
		command &= 0x7;
		if ((id == 0) || (id == eeprom_id)) {
			fifo_put(command);
		}
#ifdef USE_LED_FOR_USART_DEBUG
		LED_PIN ^= 0x1;
#endif
	}
}

void my_usart_open() {
	SPBRG = 32; // 20MHz => 9600 baud (BRGH = 0)
	TXSTAbits.BRGH = 0; // (0 = low speed)
	TXSTAbits.SYNC = 0; // (0 = asynchronous)
	
	// SPEN - Serial Port Enable Bit 
	RCSTAbits.SPEN = 1; // (1 = serial port enabled)

	// TXIE - USART Transmit Interupt Enable Bit
	PIE1bits.TXIE = 0; // (1 = enabled)

	// RCIE - USART Receive Interupt Enable Bit
	PIE1bits.RCIE = 1; // (1 = enabled)
	
	// TX9 - 9-bit Transmit Enable Bit
	TXSTAbits.TX9 = 0; // (0 = 8-bit transmit)
	
	// RX9 - 9-bit Receive Enable Bit
	RCSTAbits.RX9 = 0; // (0 = 8-bit reception)
	
	// CREN - Continuous Receive Enable Bit
	RCSTAbits.CREN = 1; // (1 = Enables receiver)
	
	// TXEN - Trasmit Enable Bit
	TXSTAbits.TXEN = 1; // (1 = transmit enabled)
}


void wait_for_command() {
	while (!fifo_in_use()) {
			// wait	
	}
	fifo_get_optimized(&command);
}

void led_debug() {
	sleep_ms(20);
	LED_PIN = 1;
	sleep_ms(80);
	LED_PIN = 0;
	sleep_ms(20);
}

unsigned char fifo_in_use() {
	return fifo_head - fifo_tail;
}

unsigned char fifo_put(command_t c) {
	if (fifo_in_use() != QUEUE_SIZE) {
		fifo_buffer[fifo_head++ % QUEUE_SIZE] = c;
		return 1;
	}
	else {
		return 0;
	}
}

unsigned char fifo_get(command_t *c) {
	if (fifo_in_use() != 0) {
		*c = fifo_buffer[fifo_tail++ % QUEUE_SIZE];
		return 1;
	}
	else {
		return 0;
	}
}

unsigned char fifo_snoop(command_t *c, unsigned char pos) {
	if (fifo_in_use() > (pos)) {
		*c = fifo_buffer[(fifo_tail + pos) % QUEUE_SIZE];
		return 1;
	}
	else {
		return 0;
	}
}

unsigned char fifo_get_optimized(command_t *c) {
	unsigned char a, b;
	unsigned char i, j;
	unsigned char in_use;
	unsigned char cmd_next_num;
	unsigned char cmd_next_nums_to_remove;
	
	cmd_next_num = 0;
	in_use = fifo_in_use();
	if (in_use >= dias_num) {
		// count them...
		for (i = 0; i < in_use; i++) {
			fifo_snoop(&a, i);
			if (a == CMD_FORWARD) {
				cmd_next_num++;
			}
		}
		cmd_next_nums_to_remove = cmd_next_num - (cmd_next_num % dias_num);
		
		// remove this many CMD_FORWARD's
		while (cmd_next_nums_to_remove > 0) {
			for (i = fifo_in_use(); i > 1; i--) {
				fifo_snoop(&a, (i - 1));
				if ((a == CMD_FORWARD) && (cmd_next_nums_to_remove > 0)) {
					// remove it
					j = i;
					while (j > 1) {
						fifo_buffer[fifo_tail + j - 1] = fifo_buffer[fifo_tail + (j - 1 - 1)];
						j--;
					}
					fifo_tail++;
					cmd_next_nums_to_remove--;
				}
			}
		}
	}
	
	// eliminate fade in/fade out followed by fade out/fade in
	while ((fifo_in_use() >= 2) && fifo_snoop(&a, 0) && fifo_snoop(&b, 1)) {
		if (((a == CMD_FADE_IN) && (b == CMD_FADE_OUT)) || ((a == CMD_FADE_OUT) && (b == CMD_FADE_IN))) {
			fifo_get(c);
			fifo_get(c);
		}
		else {
			break;
		}
	}
	if (fifo_get(c)) {
		return 1;
	}
	else {
		return 0;
	}
}

unsigned char read_EEPROM(unsigned char address) {
	EECON1 = 0;									//ensure CFGS = 0 and EEPGD = 0 
	EEADR = address;
	EECON1bits.RD = 1;
	return(EEDATA);
}
 
void write_EEPROM(unsigned char address, unsigned char d) {
	char save_int;
	save_int = INTCON;					//save interrupt status
	EECON1 = 0;									//ensure CFGS=0 and EEPGD=0
	EECON1bits.WREN = 1;				//enable write to EEPROM
	EEADR = address;						//setup Address
	EEDATA = d;									//and data
	INTCONbits.GIE=0;						//No interrupts
	EECON2 = 0x55;							//required sequence #1
	EECON2 = 0xaa;							//#2
	EECON1bits.WR = 1;					//#3 = actual write
	INTCON = save_int;					//restore interrupts
	while(!PIR2bits.EEIF) {
		//wait until finished
	}
	EECON1bits.WREN = 0;				//disable write to EEPROM
}
